#pragma once
#include <iostream>
#include <vector>

using std::cout;
using std::endl;
using std::vector;

template <typename T>
class Grid
{
public:
	//Grid(void);
	Grid(int r, int c) : rows(r), cols(c) {
		spaces = vector<vector<T*>>();
		spaces.resize(c, vector<T*>(r));

		for (int i = 0; i < r; i++) {
			for (int j = 0; j < c; j++) {
				spaces[i][j] = new T(i, j);

				// Print out the coordinates for testing
				cout << "x: " << i << " y: " << j << endl;
			}
		}
	};
	//~Grid(void);

	// Operator Overloads
	T* operator [](int index) { return spaces[i]; };

	// Functions
	void createWall(int x, int y) {
		spaces[x][y] = nullptr;
	};

	bool isWall(int x, int y) {
		return spaces[x][y] == nullptr;
	};

	vector<T*>* getNeighbors(T* n) {
		vector<T*>* neighbors = new vector<T*>();
		int currentX, currentY;

		for (int i = -1; i <= 1; i++) {
			for (int j = -1; j <= 1; j++) {
				// Don't look at the base node
				if (!(i == 0 && j == 0)) {
					// Look at a neighbor
					currentX = n->x + i;
					currentY = n->y + j;

					// Check if in bounds
					if (currentX >= 0 && currentX <= cols &&
						currentY >= 0 && currentY <= rows) {
						// Check if not a wall
						if (!isWall(currentX, currentY)) {
							// We're all set, add it to the list of neighbors
							neighbors->push_back(spaces[currentX][currentY]);
						}
					}
				}
			}
		}

		return neighbors;			
	};

	vector<T*>* astar(int sX, int sY, int gX, int gY)
	{
		T* startNode = spaces[sX][sY];
		T* goalNode = spaces[gX][gY];

		//open list and closed list
		vector<T*>* openList = new vector<T*>();
		vector<T*>* closedList = new vector<T*>();

		T* current;
		int tentativeF, tentativeG;

		vector<T*>* neighbors;
		T* neighbor;
		
		// Add starting node to open list
		openList->push_back(startNode);
		startNode->g = 0;
		startNode->f = getHeuristic(startNode, goalNode);

		while (!openList->empty()) {
			current = getLowestNode(openList);

			// Paul, I overloaded == in Node to check equivalency of location.
			if (current == goalNode)
				return reconstructPath(startNode, goalNode);

			// Remove current from openList (right now assuming it will always be at the front)
			openList->erase(openList->begin());
			// Add current to closedList
			closedList->push_back(current);

			// Iterate through neighbors
			neighbors = getNeighbors(current);

			
			for (vector<T*>::iterator it = neighbors->begin(); it != neighbors->end(); it++) {
				neighbor = *it;

				
				tentativeG = current->g + (neighbor - current);
				tentativeF = tentativeG + getHeuristic(startNode, goalNode);

				// TODO: optimize this search, it's most likely super slow
				// TODO: find a way to not use continue
				if (find(closedList->begin(), closedList->end(), neighbor) != closedList->end() && 
					tentativeF >= neighbor->f)
					continue;

				// TODO: optimize this search, it's most liekly super slow
				if (find(openList->begin(), openList->end(), neighbor) == openList->end() && tentativeF < neighbor->f) {
					// Assign the current node as the parent of this neighbor
					neighbor->parent = current;
					neighbor->g = tentativeG;
					neighbor->f = tentativeF;

					// TODO: optimize this search, it's most liekly super slow
					if (find(openList->begin(), openList->end(), neighbor) != openList->end())
						openList->push_back(neighbor);
				}
				
				//The other search is in here. Cant get the errors to go away though. been trying for hours.
				/*
				//If not in the closed set and walkable = true, add to open set
				if(neighbor->walkable == true && find(neighbors->begin(), neighbors->end(), neighbor))
				{
					//If the current node is not on the openList
					if(find(openList->begin(), openList->end(), neighbor) != openList->end())
					{
						//Put it on the openList
						openList->push_back(neighbor);

						//Find and record the h and g values of the node
						neighbor->h = neighbor->findH(goalNode);
						neighbor->g = neighbor->findG(startNode);

						//Set the current node as the parent node
						neighbor->parent = current;

					}
					//If the current node is already on the openList
					else
					{
						//Check to see if the new g valu is better than the current g value
						//Determine what the current g value is
						int currentG = neighbor->findG(startNode);

						//Save what the node's current parent is
						T* currentParent = neighbor->parent;

						//Temporarily change the parent to the current node to calculate the new g value
						neighbor->parent = current;

						//Find the potential new g value
						int tempG = neighbor->findG(startNode);

						//Compare the new g value to the original g value
						//If the old one was lower, revert the parent node back to the original parent node
						if(currentG < tempG)
						{
							neighbor->parent = currentParent;
						}
						//If the new g is lower, keep the new parent node ancorrect the g value
						else
						{
							neighbor->g = tempG;
						}
					}
				}*/

				
			}

		}

		// We shouldn't get here, if we do, something went wrong.
		return nullptr;
	};

private:
	// Private Attributes
	vector<vector<T*>> spaces;

	int rows;
	int cols;

	// Get the node with the lowest F value in the open list.
	T* getLowestNode(vector<T*>* list) {
		// Setting it to some really high number so it gets overwritten the first time.
		int min = 10000;
		T* n = nullptr;
		T* lowest = nullptr;
		
		for (vector<T*>::iterator it = list->begin(); it != list->end(); it++)
			n = *it;
			if (n->f < min) {
				lowest = n;
				min = n->f;
			}

		return lowest;
	};

	int getHeuristic(T* node, T* goal) {
		return abs(node->x - goal->x) + abs(node->y - goal->y);
	};

	// Reconstruct the path found with the A* algorithm.
	vector<T*>* reconstructPath(T* startNode, T* goalNode) {
		vector<T*>* path = new vector<T*>();
		T* current = goalNode;

		path->push_back(goalNode);

		// Reconstructs the path by iteratively going backwards using the parent pointer.
		// The path is in backwards order.
		while (current != startNode) {
			path->push_back(current->parent);
			current = current->parent;
		}

		return path;
	};
};

